/***************************************************************************
 创建者: 华磊
 开始时间: 2018.12.13
 copyright: (C) 华友高科
 ***************************************************************************/
#include "laserdriverkeli.h"
#include "sdkeli_ls_udp/sdkeli_ls1207de_parser.h"
#include "sdkeli_ls_udp/sdkeli_ls_common_udp.h"


LaserDriverKeli::LaserDriverKeli(LaserDriverConfig configIn)
                                      :LaserDriverAbstract(configIn)
{
    isShutDown=false;
    initialDriver(configIn);
    createThread();
}

int LaserDriverKeli::initialDriver(LaserDriverConfig configIn)
{
    ros::NodeHandle nh;
    ros::NodeHandle pnh("~");
    bool useTCP = true;
    std::string hostname=configIn.hostname;
//    if(nhPriv.getParam("hostname", hostname,"192.168.2.80")) {
//        useTCP = true;
//    }
    std::string port=configIn.port;
//    nhPriv.param<std::string>("port", port, "2112");
    int timelimit=configIn.timelimit;
//    nhPriv.param("timelimit", timelimit, 5);
    bool subscribe_datagram=configIn.subscribe_datagram;
    int device_number=configIn.device_number;
//    nhPriv.param("subscribe_datagram", subscribe_datagram, false);
//    nhPriv.param("device_number", device_number, 0);

    parser = new sdkeli_ls_udp::CSDKeliLs1207DEParser();
    parser->SetRangeMin(configIn.range_min);
    parser->SetRangeMax(configIn.range_max);
    parser->SetTimeIncrement(0.000061722);//使用默认
    parser->SetFrameId(configIn.frame_id);
//    if (nhPriv.getParam("range_min", param))
//    {
//      parser->set_range_min(param);
//    }
//    if (nhPriv.getParam("range_max", param,10.0))
//    {
//      parser->set_range_max(param);
//    }
//    if (nhPriv.getParam("time_increment", param,0.000061722))
//    {
//      parser->set_time_increment(param);
//    }

    int result = sdkeli_ls_udp::ExitError;
    int tryCount=0;
    while(tryCount<7)
    {
        // Atempt to connect/reconnect
         sickTimDriver = new sdkeli_ls_udp::CSDKeliLsCommonUdp(hostname, port, timelimit,
                                                  parser,nh,pnh,configIn.topicName);//话题名称
         //话题名称在哪里设置　configIn.topicName? 需要修改源码，传递进去。

        result = sickTimDriver->Init();
//        LaserDriverConfig configIn;
//        configIn.frame_id="front_laser";//激光tf基座名称
//        update_laser_driver_config(configIn);
//        sickTimDriver->update_laser_driver_config(configIn.frame_id);
        if(result == sdkeli_ls_udp::ExitSuccess)
        {
            break;
        }
        else
        {
            delete sickTimDriver;
            sickTimDriver=NULL;

            if (result == sdkeli_ls_udp::ExitFatal)
              return result;

            if (ros::ok() && !subscribe_datagram && !useTCP)
              ros::Duration(1.0).sleep(); // Only attempt USB connections once per second
        }
        tryCount++;

    }
    if(result != sdkeli_ls_udp::ExitSuccess)
    {
        qDebug()<<"error, canbus updateDomainData error!";
//        addMsg(ENUM_MSG_ERROR, "RosApi", "RosApi" , 8153, -1);
        isDriverOk=false;
        return result;
    }
    return result;
}

int LaserDriverKeli::publishLaserDataLoopOnce()
{
//    int result = sdkeli_ls_udp::ExitError;
//    if(NULL!=sickTimDriver)
//    {
//        sickTimDriver->LoopOnce();//加多一次，可能反应慢
//        result =sickTimDriver->LoopOnce();
//    }
//    else
//    {

//    }

//    if(sdkeli_ls_udp::ExitSuccess==result)
//    {
//        return 1;
//    }
//    return -1;
    return 1;
}

int LaserDriverKeli::shutDownDriver()
{
    qDebug()<<"shutDownDriver, LaserDriverKeli";
    isShutDown=true;
    if(NULL!=sickTimDriver)
    {
        delete sickTimDriver;
        delete parser;
    }
    return 1;
}

bool LaserDriverKeli::isDriverStatusOk()
{
    return isDriverOk;
}

void* LaserDriverKeli::threadRun(void *)
{
    printf("LaserDriverKeli theread start run !!!");
#if 0
    int max_cpus = sysconf(_SC_NPROCESSORS_ONLN);
    printf("Max CPUs = %d\n", max_cpus);

    /* lock all memory (prevent swapping) */
    if (mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
        printf("printf: mlockall\n");
    }

    set_latency_target();
    set_cpu(0);
#endif

    struct sched_param param;
    param.__sched_priority = 30;
    sched_setscheduler(0,SCHED_FIFO,&param);
    //setStackSize(81920);
    int policy, priority;
    pthread_getschedparam(pthread_self(), &policy, (struct sched_param *)&priority);
//    if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
//        qDebug("mlockall failed");
//    }
//    clock_gettime( CLOCK_MONOTONIC, &cycleTimespec );

   // qDebug()<<"this=="<<this;
   // pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL); //允许退出线程
    //pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL); //设置立即取消

    while(false==isShutDown)
    {
       // pthread_testcancel();/*the thread can be killed only here*/
//         QThread::msleep(1000);
            if(NULL!=sickTimDriver)
            {
        //        sickTimDriver->loopOnce();//加多一次，可能反应慢.不能加多，会把customtime 拖到１３６ｍｓ
                sickTimDriver->LoopOnce();
            }

    }
    printf("LaserDriverKeli thread quit!!!");
}

void* LaserDriverKeli::threadTask( void* classPtr )
{
    qDebug()<<"LaserDriverKeli thread run-------------------------";
    return ((LaserDriverKeli*)classPtr)->threadRun(NULL);
}

void LaserDriverKeli::createThread()
{
    int ret=pthread_create( &threadId, NULL, &LaserDriverKeli::threadTask,this);
    qDebug()<<"LaserDriverKeli createThread-------------------------";
}

